package edu.gatech.cc.liam.geometry.linear;
import java.util.ArrayList;
import java.util.Arrays;
 
public class AdditiveBasisHyperplane {

	public ArrayList<NPoint> basisVectors;
	public NPoint centerPoint;

	public AdditiveBasisHyperplane(NPoint centerPoint) {
		super();
		basisVectors = new ArrayList<NPoint>();
		this.centerPoint = centerPoint;
	}

	public boolean areVectorsNormal() {
		for(int i=0; i<basisVectors.size(); i++) {
			for(int k=i+1; k<basisVectors.size(); k++) {
				if(basisVectors.get(i).dotProduct(basisVectors.get(k)) != 0.0)
					return false;
			}
		}
		return true;
	}
	
	public void addBasisVectorAndNormalize(NPoint theNewVector) {
		NPoint normalizedNewVector = theNewVector.clone();
		for(NPoint aBasis : basisVectors){
			double scalarProjection = aBasis.dotProduct(normalizedNewVector);
			NPoint redundantPortion = NPoint.scalarMult(aBasis, -scalarProjection);
			normalizedNewVector.add(redundantPortion);
		}
		normalizedNewVector.normalize();
		this.basisVectors.add(normalizedNewVector);
 	}
	
	public NPoint project(NPoint aVector) {
		double[] newVec = new double[basisVectors.size()];
		for(int i=0; i<basisVectors.size(); i++) {
			newVec[i] = aVector.dotProduct(basisVectors.get(i).values);
		}
		NPoint newNormalVec = new NPoint(newVec);
		newNormalVec.normalize();	
		return newNormalVec;
	}
	
	public Halfspace project(Halfspace originalHyper) {
		NPoint newNormalVec = project(new NPoint(originalHyper.normal));
		
		double[] zeros = new double[originalHyper.normal.length];
		Arrays.fill(zeros, 0.0);
		NPoint newNormalOriginalBasis = new NPoint(zeros);
		for(int i=0; i<basisVectors.size(); i++) {
			newNormalOriginalBasis.add(NPoint.scalarMult(basisVectors.get(i), newNormalVec.values[i]));
		}
		
		double centerPointDistFromOriginalHyper = originalHyper.pointDistanceFromHyperplane(this.centerPoint.values);
		double normalOverlap = newNormalOriginalBasis.dotProduct(originalHyper.normal);
		double numberOfVectorStepsNeeded = centerPointDistFromOriginalHyper / normalOverlap;
		// Vector is normalized so length = 1 so numberOfVectorStepsNeeded = offset
		return new Halfspace(newNormalVec.values, numberOfVectorStepsNeeded);
	}
	

	public static void main(String[] args) {
		double[] normal = {1.0, -1.0, 0.0};
		double[] center = {2.0, 0.0, 0.0};
		double[] basis1 = {1.0, 1.0, 0.0};
		double[] basis2 = {-1.0, 1.0, 0.0};
		Halfspace a3dHyper = new Halfspace(normal, 0.0);
		AdditiveBasisHyperplane aSub = new AdditiveBasisHyperplane(new NPoint(center));
		aSub.addBasisVectorAndNormalize(new NPoint(basis1));
		aSub.addBasisVectorAndNormalize(new NPoint(basis2));
		
		Halfspace projectedHyper = aSub.project(a3dHyper);
		System.out.println("Projection: " + projectedHyper);
	}	
}
